
#include <Wire.h>
#include "peterscope.h"


int MPU6050_read(int start, uint8_t *buffer, int size);
int MPU6050_write(int start, const uint8_t *pData, int size);
int MPU6050_write_reg(int reg, uint8_t data);




/**
 * *********************
 * Public functions
 ***********************
 **/
 
 /**
  * Init MPU, awake it from sleep mode
  */ 
void ps_init()
{
	int error;
	uint8_t c;
	
	Wire.begin();

	/*
	default at power-up:
    Gyro at 250 degrees second
    Acceleration at 2g
    Clock source at internal 8MHz
    The device is in sleep mode.
	*/

	//dummy read, we doesn't do nothing really interesting with that
	error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
	
	// Clear the 'sleep' bit to start the sensor.
	MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
}
	
  
}


/**
 * Read gyro, accel and temperature
 */ 
int ps_read(ps_data * data)
{
	uint8_t buffer[14];
	int error;
	
	/* reads a chunk of 7 int16 */
	error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) buffer, 14);

	data->ax=buffer[0] | (buffer[1]<<8);
	data->ay=buffer[2] | (buffer[3]<<8);
	data->az=buffer[4] | (buffer[5]<<8);
	
	data->temp=buffer[6] | (buffer[7]<<8);
	
	data->gx=buffer[8] | (buffer[9]<<8);
	data->gy=buffer[10] | (buffer[11]<<8);
	data->gz=buffer[12] | (buffer[13]<<8);
	
	return error;
}

/**
 * *********************
 * Private functions
 ***********************
 **/


/**
* MPU6050_read
*
* This is a common function to read multiple bytes 
* from an I2C device.
*
* It uses the boolean parameter for Wire.endTransMission()
* to be able to hold or release the I2C-bus. 
* This is implemented in Arduino 1.0.1.
*
* Only this function is used to read. 
* There is no function for a single byte.
**/
int MPU6050_read(int start, uint8_t *buffer, int size)
{
  int i, n, error;

  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);
  if (n != 1)
    return (-10);

  n = Wire.endTransmission(false);    // hold the I2C-bus
  if (n != 0)
    return (n);

  // Third parameter is true: relase I2C-bus after data is read.
  Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
  i = 0;
  while(Wire.available() && i<size)
  {
    buffer[i++]=Wire.read();
  }
  if ( i != size)
    return (-11);

  return (0);  // return : no error
}


/**
* MPU6050_write
*
* This is a common function to write multiple bytes to an I2C device.
*
* If only a single register is written,
* use the function MPU_6050_write_reg().
*
* Parameters:
*   start : Start address, use a define for the register
*   pData : A pointer to the data to write.
*   size  : The number of bytes to write.
*
* If only a single register is written, a pointer
* to the data has to be used, and the size is
* a single byte:
*   int data = 0;        // the data to write
*   MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1);
**/

int MPU6050_write(int start, const uint8_t *pData, int size)
{
  int n, error;

  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);        // write the start address
  if (n != 1)
    return (-20);

  n = Wire.write(pData, size);  // write data bytes
  if (n != size)
    return (-21);

  error = Wire.endTransmission(true); // release the I2C-bus
  if (error != 0)
    return (error);

  return (0);         // return : no error
}

/**
* MPU6050_write_reg
*
* An extra function to write a single register.
* It is just a wrapper around the MPU_6050_write()
* function, and it is only a convenient function
* to make it easier to write a single register.
**/
int MPU6050_write_reg(int reg, uint8_t data)
{
  int error;

  error = MPU6050_write(reg, &data, 1);

  return (error);
}
